﻿#include <Eigen/Core>
#include <Eigen/Dense>
#include <boost/filesystem.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <memory>
#include <vector>
#include "feature_track.h"
#include <glog/logging.h>

int main(int argc, char** argv) {
  if (argc < 3) {
    std::cout << "Usage: run_feature_track [input_path] [output_path]"
              << std::endl;
    return -1;
  }

  std::string input_path = std::string(argv[1]);
  std::string output_dir = std::string(argv[2]);
  std::vector<std::tuple<std::string, int, Eigen::Matrix4d>>
      imageid_chassis_poses;
  std::map<std::string, Eigen::Matrix4d> imageid_extrinsics;

  std::string chassis_pose_txt = boost::filesystem::path(input_path)
                                     .append("imu_fusion_pose_kitti.txt")
                                     .string();

  std::string s;
  std::ifstream chassis_pose_ifs(chassis_pose_txt);
  while (getline(chassis_pose_ifs, s)) {
    if (s != "") {
      std::stringstream sstream(s);
      uint64_t camera_id = 0;
      std::string image_id;
      Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();

      sstream >> image_id >> pose(0, 0) >> pose(0, 1) >> pose(0, 2) >>
          pose(0, 3) >> pose(1, 0) >> pose(1, 1) >> pose(1, 2) >> pose(1, 3) >>
          pose(2, 0) >> pose(2, 1) >> pose(2, 2) >> pose(2, 3) /*>> camera_id*/;
      imageid_chassis_poses.emplace_back(image_id, camera_id, pose);
    }
  }
  chassis_pose_ifs.close();

  std::string extrinsic_txt = boost::filesystem::path(input_path)
                                     .append("camera_chassis_calib.txt")
                                     .string();
  std::ifstream extrinsic_ifs(extrinsic_txt);
  while (getline(extrinsic_ifs, s)) {
    if (s != "") {
      std::stringstream sstream(s);
      std::string image_id;
      Eigen::Matrix4d extrinsic = Eigen::Matrix4d::Identity();
      sstream >> image_id >> extrinsic(0, 0) >> extrinsic(0, 1) >>
          extrinsic(0, 2) >> extrinsic(0, 3) >> extrinsic(1, 0) >>
          extrinsic(1, 1) >> extrinsic(1, 2) >> extrinsic(1, 3) >>
          extrinsic(2, 0) >> extrinsic(2, 1) >> extrinsic(2, 2) >>
          extrinsic(2, 3);
      imageid_extrinsics.emplace(image_id, extrinsic);
    }
  }
  extrinsic_ifs.close();

  auto feature_tracker = std::make_shared<ville3d::FeatureTrack>();
  feature_tracker->SetOutPutDir(output_dir);

  std::string calib_path =
      boost::filesystem::path(input_path).append("cameras.txt").string();
  std::ifstream calib_ifs(calib_path);
  while (getline(calib_ifs, s)) {
    int camera_id;
    std::string camera_mode;
    double image_width, image_height, fx, fy, cx, cy;

    std::stringstream sstream(s);
    sstream >> camera_id >> camera_mode >> image_width >> image_height >> fx >>
        fy >> cx >> cy;
    feature_tracker->SetCalibParam(camera_id, fx, fy, cx, cy, image_width,
                                   image_height);
  }
  calib_ifs.close();

  for (std::size_t i = 0; i < imageid_chassis_poses.size(); i++) {
    int camera_id;
    std::string image_id;
    Eigen::Matrix4d chassis_pose;
    std::tie(image_id, camera_id, chassis_pose) = imageid_chassis_poses[i];
    if (imageid_extrinsics.find(image_id) == imageid_extrinsics.end()) {
      continue;
    }

    Eigen::Matrix4d extrinsic = imageid_extrinsics[image_id];
    Eigen::Matrix4d image_pose = chassis_pose * extrinsic.inverse();
    std::string image_name = image_id + "_0.jpg";
    std::string image_path = boost::filesystem::path(input_path)
                                 .append("images")
                                 .append(image_name)
                                 .string();

    LOG(INFO) << "tracking : " << image_path;
    feature_tracker->ReadImage(image_path, image_id, camera_id, image_pose);
  }

  feature_tracker->TrackBuild();
  feature_tracker->Triangularize();
  feature_tracker->SupplementMatch();
  feature_tracker->Optimize();

  feature_tracker->DumpProject();
  // feature_tracker->DumpTrackPoint();
  // feature_tracker->SaveColmapToPath();
  feature_tracker->SaveOdomToFile();
  feature_tracker->SaveMapPointToFile("map_point.txt");
  feature_tracker->GenerateSfMReport("report.txt");
  return 0;
}